#include "state.h"
#include "qmath.h"
#include <math.h>

void bodeSetValue()
{
    float stepF = M_PI * 2 * STATE.bode.frequency / 10.0f * STATE.bode.step / STATE.bode.controlFrequency;
    STATE.bode.value = (int)(sinf(stepF) * STATE.bode.amplitude);//(val >> 16);
}

void bodeStep()
{
    STATE.bode.step++;

    if (STATE.bode.step >= STATE.bode.controlFrequency * 10 / STATE.bode.frequency)
    {
        STATE.bode.durationCount++;
        STATE.bode.step = 0;
    }
    if (STATE.bode.durationCount >= STATE.bode.duration) STATE.bode.step = 0;
}

void bodeCalc(int error, int out, int signal)
{
    float stepF = M_PI * 2 * STATE.bode.frequency / 10.0f * STATE.bode.stepReceived / STATE.bode.controlFrequency;

    int valueCheck = (int)(sinf(stepF) * STATE.bode.amplitude);
    if (valueCheck != signal)
    {
        portNOP();
    }

    float in = out + signal;
    float sinPeriod = sinf(stepF);
    float cosPeriod = cosf(stepF);

    STATE.bode.Re_cos_Error += error * cosPeriod;
    STATE.bode.Im_sin_Error += error * sinPeriod;
    STATE.bode.Re_cos_Out += out * cosPeriod;
    STATE.bode.Im_sin_Out += out * sinPeriod;
    STATE.bode.Re_cos_In += in * cosPeriod;
    STATE.bode.Im_sin_In += in * sinPeriod;
    STATE.bode.Re_cos_Signal += signal * cosPeriod;
    STATE.bode.Im_sin_Signal += signal * sinPeriod;

    STATE.bode.stepReceived++;

    if (STATE.bode.stepReceived >= STATE.bode.controlFrequency * 10 / STATE.bode.frequency)
    {
        STATE.bode.stepReceived = 0;
    }
}

void bodeStop()
{
    STATE.bode.value = 0;
    STATE.bode.step = 0;
    STATE.bode.stepReceived = 0;
    STATE.bode.busy = 0;
}

int bodeStart(int frequency, int amplitude, int duration, int controlFrequency)
{
    if (STATE.bode.busy) return 0;

    STATE.bode.busy = 1;
    STATE.bode.frequency = frequency;
    STATE.bode.amplitude = amplitude;
    STATE.bode.duration = duration;
    STATE.bode.controlFrequency = controlFrequency;
    STATE.bode.value = 0;
    STATE.bode.step = 0;
    STATE.bode.stepReceived = 0;
    STATE.bode.durationCount = 0;
    STATE.bode.Im_sin_Error = 0;
    STATE.bode.Im_sin_In = 0;
    STATE.bode.Im_sin_Out = 0;
    STATE.bode.Im_sin_Signal = 0;
    STATE.bode.Re_cos_Error = 0;
    STATE.bode.Re_cos_In = 0;
    STATE.bode.Re_cos_Out = 0;
    STATE.bode.Re_cos_Signal = 0;

    return 1;
}